#include "formation_avoidance/formation.h"
// #include "vector.h"
#define AGENT_ID 0

using namespace std;
using namespace Eigen;

vector<float> global_goal = vector<float>(3,0);

int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "avoidance_change1");
    ros::NodeHandle nh("~");

    const int LOOPRATE = 30;
    ros::Rate loop_rate(LOOPRATE);  
    ROS_INFO("agent %d node start-----",AGENT_ID);

    Eigen::Vector3f start_p(2.8,0,1.4);
    Eigen::Vector3f end_p(7.0,0.0,1.8);

    Formation_Agent formation_agent(AGENT_ID);
    formation_agent.init(nh);
    
    nh.getParam("/SAFETY_DISTANCE",formation_agent.SAFETY_DISTANCE);
    nh.getParam("/v",formation_agent.v);
    nh.getParam("/local_length",formation_agent.local_length);
    nh.getParam("/LAND_HEIGHT",formation_agent.LAND_HEIGHT);
    nh.getParam("/global_goal",global_goal);
    

    nh.getParam("/sendpva_count_start",formation_agent.sendpva_count_start);



    formation_agent.global_goal[0] = global_goal[0];
    formation_agent.global_goal[1] = global_goal[1];
    formation_agent.global_goal[2] = global_goal[2];

    nh.getParam("/replan_time",formation_agent.replan_time);

    cout << "formation_agent.SAFETY_DISTANCE : " << formation_agent.SAFETY_DISTANCE << endl;
    cout << "formation_agent.local_length : " << formation_agent.local_length << endl;
    cout << "formation_agent.global_goal[0] : " << formation_agent.global_goal[0] << endl;
    cout << "formation_agent.global_goal[1] : " << formation_agent.global_goal[1] << endl;
    cout << "formation_agent.global_goal[2] : " << formation_agent.global_goal[2] << endl;
    cout << "formation_agent.sendpva_count_start : " << formation_agent.sendpva_count_start << endl;

    ros::AsyncSpinner spinner(3); // Use 3 threads
    spinner.start();
    ros::waitForShutdown();

    // ros::spin();
	return 0;

}
